import math
import operator
import random
import tkinter
import copy
import numpy as np
from plan_tool.Node import Node
from plan_tool.point import point
from plan_tool.astarrun import Astar
from plan_tool.RRT_star import RRTstar
from plan_tool.RRT import RRT
from plan_tool.plotmaze import plotMaze, plot2DBase, plotsjs
from plan_tool import plotmaze
from plan_tool import Floyd
import matplotlib.pyplot as plt
import matplotlib.ticker as ticker
from matplotlib.patches import Ellipse, Circle


def obstacleCre(obstacle_num, length, width, R, startList, endList, list_init):
    obstacle = []
    for i in range(obstacle_num):
        x = 0
        y = 0  # 二维坐标，作为障碍物的圆心
        r = R  # 障碍物半径
        conflict = 1
        if len(obstacle) >= 1:
            while conflict == 1:
                y = random.uniform(100, length - 1)
                x = random.uniform(0, width - 1)
                conflict = 0
                for obs in obstacle:
                    if np.sqrt(pow(x - obs[0], 2) + pow(y - obs[1], 2)) <= r + obs[2]:
                        conflict = 1
                        break
                for j in range(len(startList)):
                    if np.sqrt(pow(x - startList[j][0], 2) + pow(y - startList[j][1], 2)) <= r:
                        conflict = 1
                        break
                    if np.sqrt(pow(x - endList[j][0], 2) + pow(y - endList[j][1], 2)) <= r:
                        conflict = 1
                        break
                    if j < len(startList) - 1:
                        if np.sqrt(pow(x - list_init[j][0], 2) + pow(y - list_init[j][1], 2)) <= r:
                            conflict = 1
                            break

        else:
            y = random.uniform(100, length - 1)
            x = random.uniform(0, width - 1)

        location = [x, y, r]

        obstacle.append(location)
    return obstacle


def getdis(point1, point2):  # 曼哈顿距离
    dis = math.sqrt(pow(point1.x - point2.x, 2) + pow(point1.y - point2.y, 2))
    return dis


# 判断point是否可通行
def isAccess(thisPoint, obstacle):
    for i in obstacle:  # 处于障碍物内
        if pow(i[0] - thisPoint[0], 2) + pow(i[1] - thisPoint[1], 2) <= pow(i[2], 2) + 2:
            return False
    return True


def Heron(a, b, c):
    p = (a + b + c) / 2
    S = math.sqrt(abs(p * (p - a) * (p - b) * (p - c)))
    h = 2 * S / a
    return h


def isSafe(nextPoint, thisPoint, obstacle):
    newNear = getdis(point(nextPoint[0], nextPoint[1]), point(thisPoint[0], thisPoint[1]))
    if newNear == 0:
        return True
    for i in obstacle:
        newDx = nextPoint[0] - i[0]
        newDy = nextPoint[1] - i[1]
        nearDx = thisPoint[0] - i[0]
        nearDy = thisPoint[1] - i[1]

        newO = math.sqrt(pow(newDx, 2) + pow(newDy, 2))
        nearO = math.sqrt(pow(nearDx, 2) + pow(nearDy, 2))
        h = Heron(newNear, newO, nearO)  # 海伦公式
        cosnew = (pow(newNear, 2) + pow(newO, 2) - pow(nearO, 2)) / (2 * newO * newNear)
        cosnear = (pow(newNear, 2) + pow(nearO, 2) - pow(newO, 2)) / (2 * nearO * newNear)  # 余弦定理

        if h <= i[2] + 0.5 and cosnew > 0 and cosnear > 0:
            return False
    return True


# 车头方向为y轴版
def formation_follower_path_other(mainPathList, startTime, endTime, followerPosition, obstaCle, length, type, loss_ugv,
                                  loss_time):
    N = len(followerPosition)
    insert_flag = 0
    if loss_time is not None:
        if loss_time == startTime:
            N = N - 1
            insert_flag = 1
    type_rad = 0
    if type == 2:
        type_rad = math.pi / 4
    if endTime is None:
        endTime = len(mainPathList)

    followerPathDict = {}
    if insert_flag == 1:
        for i in range(N + 1):
            followerPathDict[i] = []
    else:
        for i in range(N):
            followerPathDict[i] = []
    for i in range(startTime, endTime):
        # print(len(mainPathList),startTime,endTime)
        thisPoint = mainPathList[i - 1]
        xMiddle = thisPoint[0]
        yMiddle = thisPoint[1] - length
        for j in range(N):
            x = xMiddle + length * math.cos(2 * math.pi * j / N + 0.5 * math.pi + type_rad)
            y = yMiddle + length * math.sin(2 * math.pi * j / N + 0.5 * math.pi + type_rad)
            if loss_ugv is not None:
                if insert_flag == 1 and j >= loss_ugv:
                    followerPathDict[j + 1].append((x, y))
                else:
                    followerPathDict[j].append((x, y))
            else:
                followerPathDict[j].append((x, y))

    # print(followerPathDict)
    return followerPathDict


# 车头方向改变版
def formation_follower_path(mainPathList, startTime, endTime, followerPosition, obstaCle, length):
    N = len(followerPosition)
    followerPathDict = {}
    for i in range(N):
        followerPathDict[i] = []
    for i in range(len(mainPathList) - 1):
        thisPoint = mainPathList[i]
        nextPoint = mainPathList[i + 1]
        toPointDis = getdis(point(thisPoint[0], thisPoint[1]), point(nextPoint[0], nextPoint[1]))
        xMiddle = thisPoint[0] - (nextPoint[0] - thisPoint[0]) * length / toPointDis
        yMiddle = thisPoint[1] - (nextPoint[1] - thisPoint[1]) * length / toPointDis
        # 计算主路径的方向角
        directionAngle = math.atan2(nextPoint[1] - thisPoint[1], nextPoint[0] - thisPoint[0])
        for j in range(N):
            # 在相对角度的基础上加上或减去主路径的方向角
            angle = 2 * math.pi * j / N + directionAngle
            x = xMiddle + length * math.cos(angle)
            y = yMiddle + length * math.sin(angle)
            followerPathDict[j].append((x, y))

    return followerPathDict


# 获得A*栅格化地图
def get_grid(obstacle, width, length, startList, endList):
    Link_List = []
    for i in range(len(startList)):
        k = (endList[i].point.y - startList[i].point.y) / (endList[i].point.x - startList[i].point.x + 0.00001)
        b = endList[i].point.y - k * endList[i].point.x
        for j in range(int(startList[i].point.x), int(endList[i].point.x)):
            y = k * j + b
            Link_List.append((round(j, 2), round(y, 2)))
        for j in range(int(startList[i].point.y), int(endList[i].point.y)):
            x = (j - b) / k
            Link_List.append((round(x, 2), round(j, 2)))
        Link_List.append((round(startList[i].point.x, 2), round(startList[i].point.y, 2)))
        Link_List.append((round(endList[i].point.x, 2), round(endList[i].point.y, 2)))

    obstacle_way_list = []
    for i in range(len(obstacle)):

        temp_r = int(obstacle[i][2] + 2)
        for j in range(temp_r + 1):
            temp_y1 = math.sqrt(temp_r * temp_r - j * j) + obstacle[i][1]
            temp_y2 = - math.sqrt(temp_r * temp_r - j * j) + obstacle[i][1]
            obstacle_way_list.append((round(obstacle[i][0] - j, 2), round(temp_y1, 2)))
            obstacle_way_list.append((round(obstacle[i][0] + j, 2), round(temp_y1, 2)))
            obstacle_way_list.append((round(obstacle[i][0] - j, 2), round(temp_y2, 2)))
            obstacle_way_list.append((round(obstacle[i][0] + j, 2), round(temp_y2, 2)))
            if j == temp_r:
                for ttt in range(10):
                    c = j - 0.1 * ttt
                    temp_y1 = math.sqrt(temp_r * temp_r - c * c) + obstacle[i][1]
                    temp_y2 = - math.sqrt(temp_r * temp_r - c * c) + obstacle[i][1]
                    obstacle_way_list.append((round(obstacle[i][0] - c, 2), round(temp_y1, 2)))
                    obstacle_way_list.append((round(obstacle[i][0] + c, 2), round(temp_y1, 2)))
                    obstacle_way_list.append((round(obstacle[i][0] - c, 2), round(temp_y2, 2)))
                    obstacle_way_list.append((round(obstacle[i][0] + c, 2), round(temp_y2, 2)))

    abstract_grid = obstacle_way_list + Link_List
    abstract_grid = list(set(abstract_grid))
    abstract_grid_point_list_temp = []
    abstract_grid_point_list = []
    del_list = []
    for pointt in abstract_grid:
        abstract_grid_point_list_temp.append(point(pointt[0], pointt[1]))

    for pointt in abstract_grid_point_list_temp:
        if pointt.x <= 0 or pointt.x >= width or pointt.y <= 0 or pointt.y >= length:
            del_list.append(pointt)
            continue
        for ob in obstacle:
            temp_dis = getdis(pointt, point(ob[0], ob[1]))
            if temp_dis <= ob[2] + 0.3 and ob[2] != 5:
                del_list.append(pointt)
                break
    for pointt in abstract_grid_point_list_temp:
        if pointt not in del_list:
            abstract_grid_point_list.append(pointt)
    global_Node = []
    for pointt in abstract_grid_point_list:
        global_Node.append(Node(pointt))

    for i in range(len(global_Node)):
        for j in range(i + 1, len(global_Node)):
            ij_dis = getdis(global_Node[i].point, global_Node[j].point)
            if ij_dis <= 0.5:
                global_Node[i].neighbor.append(global_Node[j])
                global_Node[j].neighbor.append(global_Node[i])
    output_node = []
    for i in range(len(global_Node)):
        if global_Node[i].neighbor is not None:
            output_node.append(global_Node[i])
    return output_node


def get_grid_better(obstacle, width, length, startList, endList):
    Link_List = []
    for i in range(len(startList)):
        k = (endList[i].point.y - startList[i].point.y) / (endList[i].point.x - startList[i].point.x + 0.00001)
        b = endList[i].point.y - k * endList[i].point.x
        for j in range(int(startList[i].point.x), int(endList[i].point.x)):
            y = k * j + b
            Link_List.append((round(j, 2), round(y, 2)))
        for j in range(int(startList[i].point.y), int(endList[i].point.y)):
            x = (j - b) / k
            Link_List.append((round(x, 2), round(j, 2)))
        Link_List.append((round(startList[i].point.x, 2), round(startList[i].point.y, 2)))
        Link_List.append((round(endList[i].point.x, 2), round(endList[i].point.y, 2)))

    obstacle_way_list = []
    for i in range(len(obstacle)):

        temp_r = int(obstacle[i][2] + 2)
        need_r = temp_r / math.cos(5 / 360 * math.pi)
        long_link = math.sin(5 / 360 * math.pi) * need_r * 2 + 1
        for j in range(36):
            tempxx = round(obstacle[i][0] + math.cos(j / 360 * math.pi) * need_r, 2)
            tempyy = round(obstacle[i][1] + math.sin(j / 360 * math.pi) * need_r, 2)
            obstacle_way_list.append((tempxx, tempyy))

    abstract_grid = obstacle_way_list + Link_List
    abstract_grid = list(set(abstract_grid))
    abstract_grid_point_list_temp = []
    abstract_grid_point_list = []
    del_list = []
    for pointt in abstract_grid:
        abstract_grid_point_list_temp.append(point(pointt[0], pointt[1]))

    for pointt in abstract_grid_point_list_temp:
        if pointt.x <= 0 or pointt.x >= width or pointt.y <= 0 or pointt.y >= length:
            del_list.append(pointt)
            continue
        for ob in obstacle:
            temp_dis = getdis(pointt, point(ob[0], ob[1]))
            if temp_dis <= ob[2] + 0.3 and ob[2] != 5:
                del_list.append(pointt)
                break
    for pointt in abstract_grid_point_list_temp:
        if pointt not in del_list:
            abstract_grid_point_list.append(pointt)
    global_Node = []
    for pointt in abstract_grid_point_list:
        global_Node.append(Node(pointt))

    for i in range(len(global_Node)):
        for j in range(i + 1, len(global_Node)):
            ij_dis = getdis(global_Node[i].point, global_Node[j].point)
            if ij_dis <= long_link and isSafe([global_Node[i].point.x, global_Node[i].point.y],
                                              [global_Node[j].point.x, global_Node[j].point.y], obstacle):
                global_Node[i].neighbor.append(global_Node[j])
                global_Node[j].neighbor.append(global_Node[i])

    return global_Node


# 从机避障以及路径化简
def simple_path(obstacle, obstacle_swollon, pathDict, Field_width, Field_length, formation_change_time, speed,
                loss_ugv):
    key_time = []
    time_list = []
    leak_path_dict = {}
    obstacle.append(obstacle_swollon[len(obstacle_swollon) - 1])
    for i in range(len(pathDict[0])):
        time_list.append(i)
    for key in pathDict:
        ugv_path = pathDict[key]
        for i in range(len(ugv_path) - 1):
            if ugv_path[i][0] > Field_width:
                ugv_path[i] = (Field_width - 1, ugv_path[i][1])
            elif ugv_path[i][0] < 0:
                ugv_path[i] = (1, ugv_path[i][1])
            if ugv_path[i][1] > Field_length:
                ugv_path[i] = (ugv_path[i][0], Field_length - 1)
            elif ugv_path[i][1] < 0:
                ugv_path[i] = (ugv_path[i][0], 1)  # 修正出界情况
            key_time.append(len(ugv_path) - 1)
            if isAccess(ugv_path[i], obstacle_swollon) and not isAccess(ugv_path[i + 1], obstacle_swollon):
                key_time.append(i)
            if isAccess(ugv_path[i + 1], obstacle_swollon) and not isAccess(ugv_path[i], obstacle_swollon):
                key_time.append(i + 1)  # 障碍物边缘点

            ob_flag = 0
            for ob in obstacle_swollon:  # 所有不在障碍物，但是转角过大的点
                if getdis(point(ugv_path[i][0], ugv_path[i][1]), point(ob[0], ob[1])) <= ob[2] + 2:
                    ob_flag = 1
                    break

            if ob_flag == 0:
                AB = (ugv_path[i][0] - ugv_path[i - 1][0], ugv_path[i][1] - ugv_path[i - 1][1])
                BC = (ugv_path[i + 1][0] - ugv_path[i][0], ugv_path[i + 1][1] - ugv_path[i][1])
                if (AB[0] != 0 or AB[0] != 0) and (BC[0] != 0 or BC[0] != 0):
                    dot_product = AB[0] * BC[0] + AB[1] * BC[1]
                    length_AB = math.sqrt(AB[0] ** 2 + AB[1] ** 2)
                    length_BC = math.sqrt(BC[0] ** 2 + BC[1] ** 2)
                    angle = math.acos(round(dot_product / (length_AB * length_BC), 2)) * 180 / math.pi
                    if angle >= 50:
                        key_time.append(i)

    for i in range(len(formation_change_time) - 1):
        if i == 0:
            key_time.append(0)
        else:
            tit = formation_change_time[i] - 2
            # print(tit, formation_change_time)
            if isAccess(ugv_path[tit], obstacle_swollon):
                key_time.append(tit)  # 编队变化时

    key_time = list(set(key_time))
    key_time.sort()  # 清除冗余时间点并顺序递增排列

    for key in pathDict:
        select_path = []
        ugv_path = pathDict[key]  # key车对应路径
        for t in key_time:  # 遍历该所有关键时间，作为关键点
            select_path.append(ugv_path[t])
        leak_path_dict[key] = select_path

    RRT_dict_error = {}
    for key in leak_path_dict:
        path = leak_path_dict[key]
        RRT_dict_error[key] = []
        if key == loss_ugv:
            temp_obstacle = obstacle_swollon.pop(-1)  # 丢失节点不用在意
        near_node = []

        for i in range(len(path) - 1):  # 遍历key车对应的路径点
            if i >= 1:
                if path[i] == path[i - 1] and path[i] != path[i + 1]:
                    if isAccess(path[i], obstacle_swollon) and isAccess(path[i + 1], obstacle_swollon):
                        startForObstacle = path[i]
                        endForObstacle = path[i + 1]
                        startnode = Node(point(startForObstacle[0], startForObstacle[1]))
                        endnode = Node(point(endForObstacle[0], endForObstacle[1]))
                        startnode.neighbor.append(endnode)
                        endnode.neighbor.append(startnode)
                        near_node.append([startnode, endnode, [i, i + 1]])

            if isAccess(path[i], obstacle_swollon) == True and isAccess(path[i + 1],
                                                                        obstacle_swollon) == False:  # 该节点的下一个点在障碍物内，但自身不在
                startForObstacle = path[i]
                temp = i + 1
                while not isAccess(path[temp], obstacle_swollon):
                    if temp == len(path) - 1:  # 最后一个点在障碍物内
                        break
                    else:
                        temp = temp + 1
                endForObstacle = path[temp]  # 障碍物外的第一个点
                if not isAccess(path[temp], obstacle_swollon):  # 如果
                    continue
                startnode = Node(point(startForObstacle[0], startForObstacle[1]))
                endnode = Node(point(endForObstacle[0], endForObstacle[1]))  # 获得障碍物两侧的两点
                startnode.neighbor.append(endnode)
                endnode.neighbor.append(startnode)
                near_node.append([startnode, endnode, [i, temp]])

            if isAccess(path[i], obstacle_swollon) == True and isAccess(path[i + 1], obstacle_swollon) == True:
                if not isSafe(path[i + 1], path[i], obstacle_swollon) or not isSafe(path[i], path[i + 1],
                                                                                    obstacle_swollon):
                    startForObstacle = path[i]
                    endForObstacle = path[i + 1]
                    startnode = Node(point(startForObstacle[0], startForObstacle[1]))
                    endnode = Node(point(endForObstacle[0], endForObstacle[1]))
                    startnode.neighbor.append(endnode)
                    endnode.neighbor.append(startnode)
                    near_node.append([startnode, endnode, [i, i + 1]])
                else:
                    middle_point = ((path[i + 1][0] + path[i][0]) / 2, (path[i + 1][1] + path[i][1]) / 2)
                    if not isAccess(middle_point, obstacle_swollon):
                        startForObstacle = path[i]
                        endForObstacle = path[i + 1]
                        startnode = Node(point(startForObstacle[0], startForObstacle[1]))
                        endnode = Node(point(endForObstacle[0], endForObstacle[1]))
                        startnode.neighbor.append(endnode)
                        endnode.neighbor.append(startnode)
                        near_node.append([startnode, endnode, [i, i + 1]])

        RRT_dict_error[key] = near_node
        if key == loss_ugv:
            obstacle_swollon.append(temp_obstacle)
    A_star_dict = {}
    output_dict = {}
    for key in RRT_dict_error:
        if key == loss_ugv:
            tt = obstacle.pop(-1)

        ugv_new_rrt_list = RRT_dict_error[key]
        num_for_rebuilt_point = len(ugv_new_rrt_list)
        A_star_dict[key] = []
        this_ugv_time_list = []
        for i in range(len(key_time)):
            this_ugv_time_list.append(key_time[i])

        startList = []
        endList = []
        for i in range(num_for_rebuilt_point):
            startnodee = ugv_new_rrt_list[i][0]
            startnodee.point.x = round(startnodee.point.x, 1)
            startnodee.point.y = round(startnodee.point.y, 1)
            endnodee = ugv_new_rrt_list[i][1]
            endnodee.point.x = round(endnodee.point.x, 1)
            endnodee.point.y = round(endnodee.point.y, 1)
            startList.append(startnodee)
            endList.append(endnodee)

        Global_node_list = get_grid(obstacle, Field_width, Field_length, startList, endList)
        for i in range(num_for_rebuilt_point):
            startnode = startList[i]
            endnode = endList[i]
            temp_time_list = []
            gap = ugv_new_rrt_list[i][2]
            path_little_piece = find_way(startnode, endnode, obstacle, Global_node_list, Field_width, Field_length)
            # print(len(path_little_piece))
            piece_path = Floyd.clean(path_little_piece, obstacle)
            # print(len(piece_path), 'len(piece_path)')
            # piece_path = path_little_piece
            point_num = len(piece_path)
            start_time = this_ugv_time_list[gap[0]]
            end_time = this_ugv_time_list[gap[1]]
            for j in range(point_num - 1):
                temp_time_list.append(start_time + (end_time - start_time) / (point_num - 1) * j)
            temp_time_list.append(end_time)
            A_star_dict[key].append([piece_path, temp_time_list, gap])

        for i in range(len(A_star_dict[key])):
            part_insert = A_star_dict[key][i][0]
            time_insert = A_star_dict[key][i][1]
            start_insert = 0
            end_insert = 0
            for j in range(len(this_ugv_time_list)):
                if this_ugv_time_list[j] == time_insert[0]:
                    start_insert = j
                if this_ugv_time_list[j] == time_insert[len(time_insert) - 1]:
                    end_insert = j

            part1 = []
            time1 = []
            for j in range(start_insert):
                part1.append(leak_path_dict[key][j])
                time1.append(this_ugv_time_list[j])
            part3 = []
            time3 = []
            for j in range(end_insert + 1, len(leak_path_dict[key])):
                part3.append(leak_path_dict[key][j])
                time3.append(this_ugv_time_list[j])

            leak_path_dict[key] = part1 + part_insert + part3
            this_ugv_time_list = time1 + time_insert + time3

        output_dict[key] = [leak_path_dict[key], this_ugv_time_list]
        # print(len(leak_path_dict[key]),len(this_ugv_time_list))
        if key == loss_ugv:
            obstacle.append(tt)

    # print(output_dict)
    return leak_path_dict, key_time, RRT_dict_error, output_dict, Global_node_list


def find_way(startNode, endNode, obstacle, Global_node_list, Field_width, Field_length):
    # plotsjs(obstacle, {1:[endNode.point]}, Field_width, Field_length, Global_node_list)
    startnode = None
    endnode = None
    # print(point_pattern[c],point_pattern[c])
    for node in Global_node_list:
        node.init()
        if node.point.x == startNode.point.x and node.point.y == startNode.point.y:
            startnode = node
        if node.point.x == endNode.point.x and node.point.y == endNode.point.y:
            endnode = node

    if startnode == None:
        startnode = Node(point(startNode.point.x, startNode.point.y))
        Global_node_list.append(startnode)
    if endnode == None:
        endnode = Node(point(endNode.point.x, endNode.point.y))
        Global_node_list.append(endnode)

    # print(startnode.neighbor)
    # print(endnode.neighbor)
    aStar = Astar(obstacle, startnode, endnode, Field_width, Field_length, Global_node_list)
    path = aStar.run()
    pathList = []
    if not path:
        # print("wocao", startnode.point.x, startnode.point.y, "end:", endnode.point.x, endnode.point.y)
        step = 2.5  # 1秒前进的为RRT算法步长
        mostAngle = 45
        Pg = 0.7
        rrt = RRT(obstacle, startnode, endnode, Field_width, Field_length, step, mostAngle, Pg)
        pathh = rrt.run()
        for i in pathh:
            pathList.append((i.point.x, i.point.y))

    else:
        for i in path:
            pathList.append((i.point.x, i.point.y))

    pathList.reverse()
    return pathList


def Draw_circle(thisPoint, length, output_dict, rad_speed, time, obstacles, N):
    d = 0
    length = length + 5
    silent_time = output_dict[0][1][len(output_dict[0][1]) - 1]
    dict_in_circle = {}
    for i in range(time):
        xMiddle = thisPoint[0]
        yMiddle = thisPoint[1] - length
        for j in range(N):
            temp_length = length
            while True:
                x = round((xMiddle + temp_length * math.cos(2 * math.pi * j / N + 0.5 * math.pi + rad_speed * i)) / 10,
                          3)
                y = round((yMiddle + temp_length * math.sin(2 * math.pi * j / N + 0.5 * math.pi + rad_speed * i)) / 10,
                          3)

                dis = 0
                this_ob = None
                for ob in obstacles:
                    if math.sqrt(pow(x * 10 - ob[0], 2) + pow(y * 10 - ob[1], 2)) <= ob[2] + 5:
                        dis = math.sqrt(pow(x * 10 - ob[0], 2) + pow(y * 10 - ob[1], 2))
                        this_ob = ob
                        break
                if dis == 0:

                    break
                else:
                    temp_length = temp_length - this_ob[2] * 0.1

            add_time = silent_time + i
            add_pose = (x, y)
            if i >= 2:
                output_dict[j][0].append(add_pose)
                output_dict[j][1].append(add_time)

    return output_dict


def formation(Field_width=100,  # 短边边长 单位分米
              Field_length=400,  # 长边边长 单位分米
              obstacle_num=5,  # 障碍物数量，有动捕时无效
              obstacle=None,  # 动捕障碍物信息格式[[x,y,r]...[x,y,r]]
              radius=10,  # 障碍物半径 半径分米
              speed=2,  # 速度 2分米每秒 0.2米每秒
              speed_limit=12,  # ugv速度上限，1.5米每秒,即15 dm/s
              formation_times=3,  # 3秒形成编队
              ugv_num=4,  # 默认四车编队
              edge_length=15,  # 编队距中心点距离,默认为 15 dm
              loss_ugv=None,  # 该编号节点丢失,只支持单节点丢失，如果设置此处，formation_type必须含有编队丢失信息
              formation_change_time=None,  # 编队变换时间列表,必须与formation_type数目匹配！！
              formation_type=None,  # 优先级最高！编队在该时间点的类型设定,3、4、5....N为正N边形，尖头为超前方向，2为正方形方头朝前，1为一字型编队,数字跨越则不生效
              startPointList=None,  # 编队起点终点设置,从动补中获得,第一个车默认为主车
              formation_startPoint=None,  # 编队开始时主车坐标 默认开始时为菱形
              endPoint=None,  # 主车终点设置
              baseLine=None,  # yanxianbiandui
              Map_type=None,  # 地图类型"MotionCapture" or "UWB"
              Add_circle=False,  # moweihuayuan
              isDrawMap=False  # shifouhuatu
              ):
    # 初始化
    if Map_type is None:
        Map_type = "UWB"
    if formation_startPoint is None:
        formation_startPoint = [50, 25]

    if abs(formation_startPoint[0]) >= Field_width:
        formation_startPoint[0] = (Field_width - 1) * (formation_startPoint[0] / abs(formation_startPoint[0]))
    if formation_startPoint[0] < 0 and Map_type == "UWB":
        formation_startPoint[0] = 1
    if abs(formation_startPoint[1]) >= Field_length:
        formation_startPoint[1] = (Field_length - 1) * (formation_startPoint[1] / abs(formation_startPoint[1]))
    if formation_startPoint[1] < 0 and Map_type == "UWB":
        formation_startPoint[1] = 1

    if endPoint is None:
        endPoint = [45, 399]
    if abs(endPoint[0]) >= Field_width:
        endPoint[0] = (Field_width - 1) * (endPoint[0] / abs(endPoint[0]))
    if endPoint[0] < 0 and Map_type == "UWB":
        endPoint[0] = 1
    if abs(endPoint[1]) >= Field_length:
        endPoint[1] = (Field_length - 1) * (endPoint[1] / abs(endPoint[1]))
    if endPoint[1] < 0 and Map_type == "UWB":
        endPoint[1] = 1

    if formation_type is None:
        formation_type = [ugv_num, ugv_num - 1, ugv_num]
    if formation_change_time is None:
        formation_change_time = [1, 80, 150]
    formation_change_time.append(None)

    # 根据formation_type列表生成其余对应信息
    ugv_num = max(formation_type)
    if startPointList is None:
        startPointList = [[15, 1]]
        for i in range(ugv_num - 1):
            startPointList.append([random.randint(1, Field_width - 1), 1])

    endList = []
    for i in range(len(startPointList)):
        endList.append(endPoint)

    if loss_ugv == "random":
        for i in range(1, len(formation_type)):
            if formation_type[i - 1] - formation_type[i] == 1 and formation_type[i] != 2 and formation_type[i - 1] != 2:
                loss_ugv = random.randint(1, ugv_num - 1)
                if loss_ugv == 3:
                    loss_ugv = 2
        if loss_ugv == "random":
            loss_ugv = None

    # RRT算法求主路径
    step = speed  # 1秒前进的为RRT算法步长
    mostAngle = 45
    Pg = 0.7
    main_car_startPoint = startPointList[0]

    # 编队形成位置构型初始化
    formation_start_list = []
    xMiddle = formation_startPoint[0]
    yMiddle = formation_startPoint[1] - Field_width
    for j in range(ugv_num):
        x = xMiddle + Field_width * math.cos(2 * math.pi * j / ugv_num + 0.5 * math.pi)
        y = yMiddle + Field_width * math.sin(2 * math.pi * j / ugv_num + 0.5 * math.pi)
        formation_start_list.append([x, y])

    # 模拟障碍物生成
    if obstacle is None:
        obstacle = obstacleCre(obstacle_num, Field_length, Field_width, radius, startPointList, endList,
                               startPointList)
    obstacle_swollen_main = []
    init_obstacle = []
    for i in range(len(obstacle)):
        obstacle_swollen_main.append([obstacle[i][0], obstacle[i][1], obstacle[i][2] + 2])
        init_obstacle.append([obstacle[i][0], obstacle[i][1], obstacle[i][2]])

    center_map_point = None
    if Map_type == "MotionCapture":
        center_map_point = [int(Field_width) - 1, int(Field_length) - 1]
        endPoint[0] = endPoint[0] + center_map_point[0]
        endPoint[1] = endPoint[1] + center_map_point[1]
        for i in range(len(formation_start_list)):
            formation_start_list[i][0] = formation_start_list[i][0] + center_map_point[0]
            formation_start_list[i][1] = formation_start_list[i][1] + center_map_point[1]
        for i in range(len(startPointList)):
            startPointList[i][0] = startPointList[i][0] + center_map_point[0]
            startPointList[i][1] = startPointList[i][1] + center_map_point[1]
        main_car_startPoint = startPointList[0]
        for i in range(len(init_obstacle)):
            init_obstacle[i][0] = init_obstacle[i][0] + center_map_point[0]
            init_obstacle[i][1] = init_obstacle[i][1] + center_map_point[1]
        for i in range(len(obstacle)):
            obstacle[i][0] = obstacle[i][0] + center_map_point[0]
            obstacle[i][1] = obstacle[i][1] + center_map_point[1]
        for i in range(len(obstacle_swollen_main)):
            obstacle_swollen_main[i][0] = obstacle_swollen_main[i][0] + center_map_point[0]
            obstacle_swollen_main[i][1] = obstacle_swollen_main[i][1] + center_map_point[1]
        Field_width = 2 * Field_width
        Field_length = 2 * Field_length

    # 生成主车路径
    rrt = RRT(obstacle_swollen_main, Node(point(formation_start_list[0][0], formation_start_list[0][1])),
              Node(point(endPoint[0], endPoint[1])), Field_width, Field_length, step, mostAngle, Pg)
    pathList = []
    dislist = []
    path = rrt.run()
    if not path:
        print("该地图未找到路径")
        return None
    else:
        for i in path:
            pathList.append((i.point.x, i.point.y))

        # pathList = Floyd.floyd(pathList, obstacle_swollen_main)
        for i in range(1, len(pathList)):
            dislist.append(getdis(point(pathList[i][0], pathList[i][1]), point(pathList[i - 1][0], pathList[i - 1][1])))
    pathList.reverse()

    # 如果有基线，替换生成的路径点
    if baseLine is not None and ugv_num != 1:
        pathList = baseLine

    for i in range(len(formation_change_time)):
        if formation_change_time[i] is not None:
            if len(pathList) < formation_change_time[i]:
                formation_change_time[i] = len(pathList) - 1

    # 除主机外，随机节点丢失
    loss_time = None
    last_type = None
    if loss_ugv is not None:
        loss_ugv_formation_start = formation_start_list[loss_ugv]
        for i in range(len(formation_type) - 1):  # 比上一个小1时节点丢失
            if formation_type[i] > formation_type[i + 1] != 2:
                loss_time = formation_change_time[i + 1]
                last_type = formation_type[i]

    # 初步编队
    pathDict = {}
    for i in range(len(formation_type)):
        if loss_ugv is not None:
            if last_type != formation_type[i]:
                if formation_type[i] == 1:
                    loss_time = loss_time
                if last_type == 4 and formation_type[i] == 2:
                    loss_time = loss_time
                else:
                    loss_time = formation_change_time[i]

        partDict = formation_follower_path_other(pathList, formation_change_time[i], formation_change_time[i + 1],
                                                 formation_start_list, obstacle, edge_length, formation_type[i],
                                                 loss_ugv, loss_time)

        for key in partDict:
            if key not in pathDict.keys():
                pathDict[key] = partDict[key]
            else:
                if len(partDict[key]) == 0:
                    # print(len(pathDict[key]))
                    point_last = pathDict[key][len(pathDict[key]) - 1]
                    l = 0
                    l0 = len(partDict[0])
                    while l != l0:
                        partDict[key].append(point_last)
                        l = len(partDict[key])
                pathDict[key] = pathDict[key] + partDict[key]

    if loss_ugv is not None:
        if (loss_ugv == 0 or loss_ugv == 3) and ugv_num == 4:  # 权宜之计
            obstacle_swollen_main.append(
                [pathDict[loss_ugv][loss_time - 2][0], pathDict[loss_ugv][loss_time - 2][1], 5])  # 不能设为5
        # else:
        #     obstacle_swollen_main.append(
        #         [pathDict[loss_ugv][loss_time - 2][0], pathDict[loss_ugv][loss_time - 2][1], 10])  # 不能设为5
    if ugv_num == 1:
        single_pathList = Floyd.clean_infinite(pathDict[0], obstacle)
        real_pathlist = []
        # print(len(single_pathList))
        for i in range(len(single_pathList) - 1):
            real_pathlist.append(single_pathList[i])
            dis_real_next_point = 0
            dis_two_single_point = math.sqrt(pow(single_pathList[i + 1][0] - single_pathList[i][0], 2) + pow(
                single_pathList[i + 1][1] - single_pathList[i][1], 2))
            while dis_real_next_point <= dis_two_single_point:
                new_real_x = round(
                    dis_real_next_point / dis_two_single_point * (single_pathList[i + 1][0] - single_pathList[i][0]) +
                    single_pathList[i][0], 2)
                new_real_y = round(
                    dis_real_next_point / dis_two_single_point * (single_pathList[i + 1][1] - single_pathList[i][1]) +
                    single_pathList[i][1], 2)
                real_pathlist.append((new_real_x, new_real_y))
                dis_real_next_point = speed + dis_real_next_point
        real_pathlist.append(single_pathList[len(single_pathList) - 1])
        single_pathList = real_pathlist
        for i in range(len(formation_change_time)):
            if formation_change_time[i] is not None:
                if len(single_pathList) < formation_change_time[i]:
                    formation_change_time[i] = len(single_pathList) - 1
        pathDict[0] = single_pathList

    path_after_simple_dict, key_time, RRT_dict, output_dict, Global_node_list = simple_path(obstacle,
                                                                                            obstacle_swollen_main,
                                                                                            pathDict,
                                                                                            Field_width, Field_length,
                                                                                            formation_change_time,
                                                                                            speed, loss_ugv)
    printfor = []
    for key in RRT_dict:
        for j in range(len(RRT_dict[key])):
            for k in range(2):
                printfor.append(RRT_dict[key][j][k])  # 调试代码
    # , RRT_dict[2], RRT_dict[3], printfor)

    for key in output_dict:
        ugv_out_put = output_dict[key]
        path_output = ugv_out_put[0]
        time_output = ugv_out_put[1]
        path_output.insert(0, startPointList[key])
        meters_ouput_path = []
        for j in range(len(path_output)):
            meters_ouput_path.append((round(path_output[j][0] / 10, 2), round(path_output[j][1] / 10, 2)))
        for j in range(len(time_output)):
            time_output[j] = time_output[j] + formation_times

        time_output.insert(0, 0)
        output_dict[key][0] = meters_ouput_path
        output_dict[key][1] = time_output
        for j in range(len(output_dict[key][1])):
            output_dict[key][1][j] = round(output_dict[key][1][j], 2)

    speed_limit = speed_limit / 10

    for key in output_dict:
        v_list = []
        del_list = []
        for i in range(1, len(output_dict[key][1]) - 1):
            if (output_dict[key][0][i + 1][0] == output_dict[key][0][i][0] == output_dict[key][0][i - 1][0] and
                    output_dict[key][0][i + 1][1] == output_dict[key][0][i][1] == output_dict[key][0][i - 1][1]):
                del_list.append(i)
        del_list.reverse()
        for i in range(len(del_list)):
            del output_dict[key][0][del_list[i]]
            del output_dict[key][1][del_list[i]]

        for i in range(len(output_dict[key][1]) - 1):
            disdentce = getdis(point(output_dict[key][0][i + 1][0], output_dict[key][0][i + 1][1]),
                               point(output_dict[key][0][i][0], output_dict[key][0][i][1]))
            time_gap = output_dict[key][1][i + 1] - output_dict[key][1][i]
            v = disdentce / time_gap
            v_list.append(v)

        other_keys_list = []
        this_ugv_time_set = []
        piece_end_time = 0
        for i in range(1, len(v_list)):
            if v_list[i] >= speed_limit + 1.0 and i > piece_end_time:
                # i点到i+1点之间超速
                temp = i + 1
                if len(other_keys_list) == 0:
                    for other_keys in output_dict:
                        if other_keys != key:
                            other_keys_list.append(other_keys)
                while True:
                    change_flag = 0
                    if temp < len(output_dict[key][1]):
                        for other_ugv in other_keys_list:
                            if output_dict[key][1][temp] not in output_dict[other_ugv][1]:
                                temp = temp + 1
                                change_flag = 1
                                break
                        if change_flag == 0:
                            # print(temp,output_dict[key][1][temp], key+1, v_list[i])
                            break
                    else:
                        temp = len(output_dict[key][1]) - 1
                        break

                piece_end_time = temp
                this_ugv_time_set.append([i, temp])

        if len(this_ugv_time_set) != 0:  # 有若干超速片段
            for i in range(len(this_ugv_time_set)):
                start_piece = this_ugv_time_set[i][0]
                end_piece = this_ugv_time_set[i][1]  # 第几个点为分段点
                total_time_gap = output_dict[key][1][end_piece] - output_dict[key][1][start_piece]  # 时间差
                total_dis_gap = 0  # 计算这段时间的总距离
                end_time = output_dict[key][1][end_piece]
                for j in range(start_piece, end_piece):
                    total_dis_gap = (getdis(point(output_dict[key][0][j][0], output_dict[key][0][j][1]),
                                            point(output_dict[key][0][j + 1][0], output_dict[key][0][j + 1][1]))
                                     + total_dis_gap)
                if total_time_gap * speed_limit <= total_dis_gap:
                    # print("全域超速，需要进行等待",key)
                    # 先对本车的时间进行计算
                    true_need_time = round(total_dis_gap / speed_limit, 2)
                    should_add_time = true_need_time - total_time_gap
                    # print(should_add_time)
                    # 汇合点之后
                    for j in range(end_piece, len(output_dict[key][1])):
                        output_dict[key][1][j] = round(output_dict[key][1][j] + should_add_time, 2)
                    # 断点中
                    for j in range(start_piece, end_piece):
                        dis_to_next = getdis(point(output_dict[key][0][j][0], output_dict[key][0][j][1]),
                                             point(output_dict[key][0][j + 1][0], output_dict[key][0][j + 1][1]))
                        next_add_time = round(dis_to_next / speed_limit, 2)
                        if j + 1 != end_piece:
                            output_dict[key][1][j + 1] = round(output_dict[key][1][j] + next_add_time, 2)
                    new_end_time = output_dict[key][1][end_piece]

                    # 其余车辆进行等待操作
                    for other_ugv in other_keys_list:
                        end_pos_in_others = None
                        for j in range(len(output_dict[other_ugv][1])):
                            if end_time == output_dict[other_ugv][1][j]:
                                end_pos_in_others = j + 1
                                break
                            if j != len(output_dict[other_ugv][1]) - 1:
                                if output_dict[other_ugv][1][j] < end_time < output_dict[other_ugv][1][j + 1]:
                                    end_pos_in_others = j + 1
                                    break
                        if end_pos_in_others is None:
                            end_pos_in_others = len(output_dict[other_ugv][1])
                        # 给插入点后的点增加固定时间
                        if end_pos_in_others != len(output_dict[other_ugv][1]):
                            for j in range(end_pos_in_others, len(output_dict[other_ugv][1])):
                                output_dict[other_ugv][1][j] = output_dict[other_ugv][1][j] + should_add_time

                        output_dict[other_ugv][0].insert(end_pos_in_others,
                                                         output_dict[other_ugv][0][end_pos_in_others - 1])
                        output_dict[other_ugv][1].insert(end_pos_in_others, new_end_time)

                else:
                    # print("可以内部协调，无需其他车辆等待",key)
                    avg_speed_for_this_situation = total_dis_gap / total_time_gap
                    for j in range(start_piece, end_piece):
                        dis_to_next = getdis(point(output_dict[key][0][j][0], output_dict[key][0][j][1]),
                                             point(output_dict[key][0][j + 1][0], output_dict[key][0][j + 1][1]))
                        next_add_time = round(dis_to_next / avg_speed_for_this_situation, 2)
                        if j + 1 != end_piece:
                            output_dict[key][1][j + 1] = output_dict[key][1][j] + next_add_time

    if Add_circle:
        radspeed = speed / edge_length
        time = 60
        output_dict = Draw_circle(endPoint, edge_length, output_dict, radspeed, time, init_obstacle, ugv_num)
        for key in path_after_simple_dict:
            temp_list = []
            for i in range(len(output_dict[key][0])):
                temp_list.append((output_dict[key][0][i][0] * 10, output_dict[key][0][i][1] * 10))
            path_after_simple_dict[key] = temp_list

    if Map_type == "MotionCapture":
        if isDrawMap is True:
            plotsjs(init_obstacle, path_after_simple_dict, Field_width, Field_length, "MotionCapture", center_map_point,
                    printfor)  # 车数量任意Global_node_list
        for key in output_dict:
            for c in range(len(output_dict[key][0])):
                output_dict[key][0][c] = (round(output_dict[key][0][c][0] - center_map_point[0] / 10, 2),
                                          round(output_dict[key][0][c][1] - center_map_point[1] / 10, 2))
        for key in output_dict:
            output_dict[key][0].pop(0)
            output_dict[key][1].pop(0)
            print("ugv_id: ", key)
            print("路径点： ", output_dict[key][0])
            print("时间戳： ", output_dict[key][1])
            print(" ")
        # plotsjs(init_obstacle, output_dict[key][0], Field_width, Field_length, "UWB", center_map_point,printfor)  # 车数量任意Global_node_list
    elif Map_type == "UWB":
        for key in output_dict:
            output_dict[key][0].pop(0)
            output_dict[key][1].pop(0)
            print("ugv_id: ", key)
            print("路径点： ", output_dict[key][0])
            print("时间戳： ", output_dict[key][1])
            print(" ")
        if isDrawMap is True:
            plotsjs(init_obstacle, path_after_simple_dict, Field_width, Field_length, "UBW", center_map_point,
                    printfor)  # 车数量任意

    return output_dict  # 输出，格式为{id:[ [(x,y),(x,y),...,(x,y)],[t1,t2,t3] ]}


def link_formation(Field_width=100,  # 短边边长 单位分米
                   Field_length=400,  # 长边边长 单位分米
                   obstacle_num=5,  # 障碍物数量，有动捕时无效
                   obstacle=None,  # 动捕障碍物信息格式[[x,y,r]...[x,y,r]]
                   radius=10,  # 障碍物半径 半径分米
                   speed=2,  # 速度 2分米每秒 0.2米每秒
                   speed_limit=12,  # ugv速度上限，1.5米每秒,即15 dm/s
                   formation_times=3,  # 3秒形成编队
                   ugv_num=4,  # 默认四车编队
                   edge_length=15,  # 编队距中心点距离,默认为 15 dm
                   loss_ugv=None,  # 该编号节点丢失,只支持单节点丢失，如果设置此处，formation_type必须含有编队丢失信息
                   formation_change_time=None,  # 编队变换时间列表,必须与formation_type数目匹配！！
                   formation_type=None,  # 优先级最高！编队在该时间点的类型设定,3、4、5....N为正N边形，尖头为超前方向，2为正方形方头朝前，1为一字型编队,数字跨越则不生效
                   startPointList=None,  # 编队起点终点设置,从动补中获得,第一个车默认为主车
                   formation_startPoint=None,  # 编队开始时主车坐标 默认开始时为菱形
                   keyPointList=None,  # 主车终点设置
                   baseLine=None,  # yanxianbiandui
                   Map_type=None,  # 地图类型"MotionCapture" or "UWB"
                   Add_circle=False,  # moweihuayuan
                   isDrawMap=False  # shifouhuatu
                   ):
    total_path_dict = {}
    if keyPointList is None:
        keyPointList = [(0, 50), (50, 50), (50, 150)]

    obstacle_ = []
    for ob in range(len(obstacle)):
        obstacle_.append([obstacle[ob][0], obstacle[ob][1], obstacle[ob][2]])
    for k in range(len(keyPointList)):
        if not isAccess(keyPointList[k], obstacle_):
            print("所选途径点在障碍物中")
            return

    obstacle_ = []
    for ob in range(len(obstacle)):
        obstacle_.append([obstacle[ob][0], obstacle[ob][1], obstacle[ob][2]])
    if not isAccess(formation_startPoint, obstacle_):
        print("起点在障碍物中")
        return
    Field_width_ = Field_width
    Field_length_ = Field_length
    obstacle_num_ = obstacle_num
    radius_ = radius
    speed_ = speed
    speed_limit_ = speed_limit
    formation_times_ = formation_times
    ugv_num_ = ugv_num
    edge_length_ = edge_length
    formation_change_time_ = formation_change_time
    formation_type_ = formation_type
    startPointList_ = startPointList
    Map_type_ = Map_type
    formation_startPoint_ = [formation_startPoint[0], formation_startPoint[1]]

    for phrase in range(len(keyPointList)):
        if phrase != 0:
            formation_startPoint_ = [keyPointList[phrase - 1][0], keyPointList[phrase - 1][1]]
            formation_change_time_ = [1]
            formation_type_ = [ugv_num]
        obstacle_ = []
        for ob in range(len(obstacle)):
            obstacle_.append([obstacle[ob][0], obstacle[ob][1], obstacle[ob][2]])

        end_point = [keyPointList[phrase][0], keyPointList[phrase][1]]
        temp_path_dict = formation(Field_width=Field_width_,  # 短边边长 单位分米
                                   Field_length=Field_length_,  # 长边边长 单位分米
                                   obstacle_num=obstacle_num_,  # 障碍物数量，有动捕时无效
                                   obstacle=obstacle_,  # 动捕障碍物信息格式[[x,y,r]...[x,y,r]]
                                   radius=radius_,  # 障碍物半径 半径分米
                                   speed=speed_,  # 速度 2分米每秒 0.2米每秒
                                   speed_limit=speed_limit_,  # ugv速度上限，1.5米每秒,即15 dm/s
                                   formation_times=formation_times_,  # 3秒形成编队
                                   ugv_num=ugv_num_,  # 默认四车编队
                                   edge_length=edge_length_,  # 编队距中心点距离,默认为 15 dm
                                   formation_change_time=formation_change_time_,  # 编队变换时间列表,必须与formation_type数目匹配！！
                                   formation_type=formation_type_,
                                   # 优先级最高！编队在该时间点的类型设定,3、4、5....N为正N边形，尖头为超前方向，2为正方形方头朝前，1为一字型编队,数字跨越则不生效
                                   startPointList=startPointList_,  # 各小车起点设置,从动补中获得,第一个车默认为主车
                                   formation_startPoint=formation_startPoint_,  # 编队开始时主车坐标 默认开始时为菱形
                                   endPoint=end_point,  # 主车终点设置
                                   Map_type=Map_type_,  # 地图类型"MotionCapture" or "UWB"
                                   isDrawMap=False  # shifouhuatu
                                   )
        if len(temp_path_dict) == 0:
            print("在前往点", phrase, "的过程中未找到路径")
            if phrase == 0:
                total_path_dict = temp_path_dict
            else:
                for key in total_path_dict:
                    total_path_dict[key][0] = total_path_dict[key][0] + temp_path_dict[key][0]
                    for j in range(len(temp_path_dict[key][1])):
                        temp_path_dict[key][1][j] = temp_path_dict[key][1][j] + total_path_dict[key][1][
                            len(total_path_dict[key][1]) - 1]
                    total_path_dict[key][1] = total_path_dict[key][1] + temp_path_dict[key][1]
            print("总路径：")
            plot_dict = {}
            for key in total_path_dict:
                print("ugv_id: ", key)
                print("路径点： ", total_path_dict[key][0])
                print("时间戳： ", total_path_dict[key][1])
                print(" ")
                plot_dict[key] = total_path_dict[key][0]
                for po in range(len(plot_dict[key])):
                    plot_dict[key][po] = (plot_dict[key][po][0] * 10, plot_dict[key][po][1] * 10)
            plotsjs(obstacle, plot_dict, Field_width, Field_length, Map_type, temp_list_to_plot=keyPointList)
            return

        if phrase == 0:
            total_path_dict = temp_path_dict
        else:
            for key in total_path_dict:
                total_path_dict[key][0] = total_path_dict[key][0] + temp_path_dict[key][0]
                for j in range(len(temp_path_dict[key][1])):
                    temp_path_dict[key][1][j] = temp_path_dict[key][1][j] + total_path_dict[key][1][
                        len(total_path_dict[key][1]) - 1]
                total_path_dict[key][1] = total_path_dict[key][1] + temp_path_dict[key][1]

    print("总路径：")
    plot_dict = {}
    for key in total_path_dict:
        print("ugv_id: ", key)
        print("路径点： ", total_path_dict[key][0])
        print("时间戳： ", total_path_dict[key][1])
        print(" ")
        plot_dict[key] = []
        for poi in range(len(total_path_dict[key][0])):
            plot_dict[key].append(total_path_dict[key][0][poi])

        for po in range(len(plot_dict[key])):
            plot_dict[key][po] = (plot_dict[key][po][0] * 10, plot_dict[key][po][1] * 10)
    plotsjs(obstacle, plot_dict, Field_width, Field_length, Map_type,temp_list_to_plot=keyPointList)  # 车数量任意
    return total_path_dict


if __name__ == '__main__':
    PATH = None

    # baseLine 示例，为一串坐标点
    width = 100  # 短边边长 单位分米
    length = 500  # 长边边长 单位分米
    speed = 2  # 速度 2分米每秒 0.2米每秒
    # formation_start_point = [50, 25]  # 编队开始时主车坐标 默认开始时为菱形

    obstacle = [[-10, 10, 3], [0, 10, 3], [20, 25, 3]]  # , [-20, 360, 10]
    formation_start_point = [0, -200]
    endpoint = [0, 400]
    startPointList = [[0, -220], [-20, -210], [40, -210], [20, -210]]

    baseLine = []

    for i in range(formation_start_point[1], int(length / 2) - 5):
        baseLine.append((formation_start_point[0], i * speed))
        # baseLine.append((10 * math.sin(i * speed / 4) + 50,i * speed))

    while PATH is None:
        # path_dict = formation(obstacle=obstacle,
        #                       Field_length=50,
        #                       Field_width=50,
        #                       edge_length=10,
        #                       ugv_num=4,
        #                       formation_change_time=[1],
        #                       formation_type=[4],
        #                       endPoint=[0, 50],
        #                       startPointList=[[-50, -50], [-50, -50], [-50, -50], [-50, -50]],
        #                       formation_startPoint=[0, -50],
        #                       Map_type="MotionCapture",
        #                       loss_ugv="random",
        #                       # Add_circle=True,
        #                       isDrawMap=True
        #                       )
        path_dict = link_formation(obstacle=obstacle,
                                   ugv_num=1,
                                   edge_length=10,
                                   keyPointList=[(-20, -20), (0, -10), (-10, 0), (-5, 15), (15, 20)],
                                   startPointList=[[-30, -30], [-30, -30], [-30, -30], [-30, -30]],
                                   formation_startPoint=[-30, -30],
                                   Map_type="MotionCapture",
                                   isDrawMap=False,
                                   Field_width=150,
                                   Field_length=100
                                   )

        # path_dict = formation(Field_width=width, Field_length=length, obstacle = obstacle, Map_type="MotionCapture",
        #                       endPoint=endpoint, startPointList=startPointList, ugv_num=1)
        # path_dict = formation(ugv_num=1)
        # print(path_dict)
        # path_dict = formation(Field_width=width, Field_length=length)
        break
